#|;;;; Created on 2010-10-18 19:50:29
(in-package :partial-order-planner)
(defun general-search (problem qing-fun &optional (cut-off nil cut-off-supplied-p))
  
  (let* ((nodes (list (make-node :state (problem-init-state problem) 
                                 :depth 0 
                                 :path-cost 0 
                                 :parent nil 
                                 :operator nil ) ()))
         (num-expanded (list-length nodes)))
    (loop do 
          (if   (eq 0 (list-length nodes))
              (return (list nil num-expanded))
              )
          (let ((node nil)
                (expanded-nodes nil)) 
            (setf node (first nodes))
            (if (eq node nil)
                (return (list nil num-expanded))
                )
            ;(print (funcall (problem-goal-test problem) (node-state node)))
            (if (funcall (problem-goal-test problem) (node-state node))
                (return (list node num-expanded))
                )
            
            ;(setf expanded-nodes nil)
            ;(print (problem-operators problem))
            (if (eq t cut-off-supplied-p)
                (if (> cut-off (node-depth node))
                    (setf expanded-nodes (expand-node node problem))
                    )
                (setf expanded-nodes (expand-node node problem))
                )
            
            (setf num-Expanded (+ num-expanded (list-length expanded-nodes)))
            (setf nodes (funcall qing-fun (rest nodes) expanded-nodes)))
          )
    )
  )

(defun uniform-cost-search (problem &optional (cut-off nil cut-off-supplied-p))
  (if (eql t cut-off-supplied-p)
      (general-search problem #'ordered-insert cut-off)
      (general-search problem #'ordered-insert)
      )
  )

(defun ordered-insert (nodes exp-node-list)
  (let ((ordered nil)
        (non-ordered nil))
    (if (equal nodes (list nil))
        (setf non-ordered exp-node-list)
        (if (equal exp-node-list (list nil))
            (setf non-ordered nodes)
            (setf non-ordered (append nodes exp-node-list))
            )
        )
    (setf ordered nil)
    (loop do
          (if (not  (eql non-ordered (list nil)))
              (return   (sort non-ordered #'< :key #'node-path-cost))
              )
          )
    )
  )

(defun expand-node (node problem)
  (let ((exp-nodes ())
        (depth (node-depth node))
        (av-op (av-ops (node-state node) (problem-operators problem))))
    (dolist (i av-op)
      (let ((new-node (make-node :state  (apply-op (node-state node) i) 
                                 :operator i
                                 :parent node
                                 :depth (1+ depth) 
                                 )
                      ))
        (setf (node-path-cost new-node) (funcall (problem-path-cost-fn  problem) node problem))
        ;(add-state (node-state new-node))
        (setf exp-nodes (cons new-node exp-nodes)))
      )
    exp-nodes)
  )

(defun  strategy-selector (strategy)
  (let ((fun nil))
    (cond 
      ;((eq strategy 'bf)(setf fun (lambda (node problem) (node-depth node))))
      ;((eq strategy 'df)(setf fun (lambda (node) (* -1 (node-depth node)))))
      ;((eq strategy 'it)(setf fun (lambda (node) (* -1 (node-depth node)))))
      ;((eq strategy 'gr1)(setf  fun #' Manhattan))
      ;((eq strategy 'gr2)(setf fun #'misplaced-tiles-h))
      ;((eq strategy 'gr3)(setf fun #'blank))
      ((eq strategy 'as1)(setf fun (lambda (node problem) (+ (node-depth node) (funcall #'search-heuristic node problem)))))
      ;((eq strategy 'as2) (setf fun (lambda (node goal-state) (+ (node-depth node) (funcall #'misplaced-tiles-h node goal-state)))))
      ;((eq strategy 'as3)(setf fun (lambda (node goal-state) (+ (node-depth node) (funcall #'blank node goal-state)))))
      (t nil)
      )
    fun
    )
  )|#